//文件名：fft.c
#include "fft.h"


float sqrt_fast(float x)
{
	long  lx;
	float temp;
	float xhalf = x * 0.5f;
	lx = *((long*)&x);
	lx = 0x5F375A86 - (lx>>1);
	temp = *((float*)&lx);
	temp = temp*(1.5f - xhalf*temp*temp);
	temp = temp*(1.5f - xhalf*temp*temp);
	temp = temp*(1.5f - xhalf*temp*temp);
	return temp * x;
}
 
//128等分余弦表
static const double cosTable[] =
{
	1.0000,0.9997,0.9988,0.9973,0.9952,0.9925,0.9892,0.9853,
	0.9808,0.9757,0.9700,0.9638,0.9569,0.9495,0.9415,0.9330,
	0.9239,0.9142,0.9040,0.8932,0.8819,0.8701,0.8577,0.8449,
	0.8315,0.8176,0.8032,0.7883,0.7730,0.7572,0.7410,0.7242,
	0.7071,0.6895,0.6716,0.6532,0.6344,0.6152,0.5957,0.5758,
	0.5556,0.5350,0.5141,0.4929,0.4714,0.4496,0.4276,0.4052,
	0.3827,0.3599,0.3369,0.3137,0.2903,0.2667,0.2430,0.2191,
	0.1951,0.1710,0.1467,0.1224,0.0980,0.0736,0.0491,0.0245,
	-0.0000,-0.0245,-0.0491,-0.0736,-0.0980,-0.1224,-0.1467,-0.1710,
	-0.1951,-0.2191,-0.2430,-0.2667,-0.2903,-0.3137,-0.3369,-0.3599,
	-0.3827,-0.4052,-0.4276,-0.4496,-0.4714,-0.4929,-0.5141,-0.5350,
	-0.5556,-0.5758,-0.5957,-0.6152,-0.6344,-0.6532,-0.6716,-0.6895,
	-0.7071,-0.7242,-0.7410,-0.7572,-0.7730,-0.7883,-0.8032,-0.8176,
	-0.8315,-0.8449,-0.8577,-0.8701,-0.8819,-0.8932,-0.9040,-0.9142,
	-0.9239,-0.9330,-0.9415,-0.9495,-0.9569,-0.9638,-0.9700,-0.9757,
	-0.9808,-0.9853,-0.9892,-0.9925,-0.9952,-0.9973,-0.9988,-0.9997,
};
 
//128等分正弦表
static const double sinTable[] = 
{
	0.0000,0.0245,0.0491,0.0736,0.0980,0.1224,0.1467,0.1710,
	0.1951,0.2191,0.2430,0.2667,0.2903,0.3137,0.3369,0.3599,
	0.3827,0.4052,0.4276,0.4496,0.4714,0.4929,0.5141,0.5350,
	0.5556,0.5758,0.5957,0.6152,0.6344,0.6532,0.6716,0.6895,
	0.7071,0.7242,0.7410,0.7572,0.7730,0.7883,0.8032,0.8176,
	0.8315,0.8449,0.8577,0.8701,0.8819,0.8932,0.9040,0.9142,
	0.9239,0.9330,0.9415,0.9495,0.9569,0.9638,0.9700,0.9757,
	0.9808,0.9853,0.9892,0.9925,0.9952,0.9973,0.9988,0.9997,
	1.0000,0.9997,0.9988,0.9973,0.9952,0.9925,0.9892,0.9853,
	0.9808,0.9757,0.9700,0.9638,0.9569,0.9495,0.9415,0.9330,
	0.9239,0.9142,0.9040,0.8932,0.8819,0.8701,0.8577,0.8449,
	0.8315,0.8176,0.8032,0.7883,0.7730,0.7572,0.7410,0.7242,
	0.7071,0.6895,0.6716,0.6532,0.6344,0.6152,0.5957,0.5758,
	0.5556,0.5350,0.5141,0.4929,0.4714,0.4496,0.4276,0.4052,
	0.3827,0.3599,0.3369,0.3137,0.2903,0.2667,0.2430,0.2191,
	0.1951,0.1710,0.1467,0.1224,0.0980,0.0736,0.0491,0.0245,
};

 
//8bit位反转
unsigned char bitrev(unsigned char i)
{
	unsigned char r=0;
	r |= (i>>0)&0x01;
	r<<= 1;
	r |= (i>>1)&0x01;
	r<<= 1;
	r |= (i>>2)&0x01;
	r<<= 1;
	r |= (i>>3)&0x01;
	r<<= 1;
	r |= (i>>4)&0x01;
	r<<= 1;
	r |= (i>>5)&0x01;
	r<<= 1;
	r |= (i>>6)&0x01;
	r<<= 1;
	r |= (i>>7)&0x01;
	return r;
}
 
 
//256点 fft运算
//输入256个采样数据,只用实数部分，虚数为0
//输出128个频域数据,已将复数转换为幅度(频谱)
void FFT256(long* pInputData, long* pOutputData)
{
	double	real[256];
	double	imag[256];
	long	swap;
	long	offset;
	double	tmp_real, tmp_imag, temp;// 临时变量, 记录实部
	double	tw1, tw2;	// 旋转因子,tw1为旋转因子的实部cos部分, tw2为旋转因子的虚部sin部分.
	long	cur_layer, gr_num, i, k, p;      
	long	step;			// 步进
	long	sample_num;		// 颗粒的样本总数(各层不同, 因为各层颗粒的输入不同) 
 
	//虚部为0
	for(i=0; i<256; i++)
	{
		imag[i] = 0;
		real[i] = 0;
	}
	
	//倒位序
	for(i=0; i<256; i++)
	{
		swap = bitrev((unsigned char)i);
		real[swap] = (float)pInputData[i];
	}
	
	// 对层循环
	for(cur_layer=1; cur_layer<=8; cur_layer++)	//2^8=256,所以循环8次
	{      
		// 求当前层拥有多少个颗粒(gr_num)
		i = 8 - cur_layer;
		gr_num=1<<i;
		//每个颗粒的输入样本数N
		sample_num = 1<<cur_layer;
		//步进. 步进是N/2
		step = sample_num/2;
		k = 0;
		//对颗粒进行循环
		for(i=0; i<gr_num; i++)
		{
			//对样本点进行循环, 注意上限和步进				
			for(p=0; p<sample_num/2; p++)
			{   
				offset = p*gr_num;
				tw1 = cosTable[offset];
				tw2 = sinTable[offset];
				tmp_real = real[k+p];
				tmp_imag = imag[k+p];
				temp = real[k+p+step];             
				//蝶形算法
				real[k+p]   = tmp_real + ( tw1*real[k+p+step] - tw2*imag[k+p+step] );
				imag[k+p]   = tmp_imag + ( tw2*real[k+p+step] + tw1*imag[k+p+step] );
				real[k+p+step] = tmp_real - ( tw1*temp - tw2*imag[k+p+step] );
				imag[k+p+step] = tmp_imag - ( tw2*temp + tw1*imag[k+p+step] );             
			}
			//开跳
			k += 2*step;
		}   
	}
	
	//对复数求平方根
	for(i=0; i<128;i++)
	{
		temp = real[i]*real[i] + imag[i]*imag[i];
		temp = sqrt_fast(temp);
		pOutputData[i] = (long)temp;
	}
}
